import sys
import time

from naoqi import ALProxy
from naoqi import ALModule
from naoqi import ALBroker
import motion

class ControlNaoModule(ALModule):
    '''
    Class for sending control commands to NAO
    '''
    
    def __init__( self,moduleName ,robotIP):
        ALModule.__init__(self,moduleName)
        #Init proxies, there should be more of these if we want to do more than just go to posture
        self.postureProxy = ALProxy("ALRobotPosture", robotIP, 9559)
        self.motionProxy = ALProxy("ALMotion", robotIP, 9559)
        self.ledProxy = ALProxy("ALLeds",robotIP, 9559)
        self.textToSpeechProxy = ALProxy("ALTextToSpeech", robotIP, 9559)
            
    def executeControl(self, functionName, functionParams):
        '''
        Method for sending the actual control command to NAO
        @param functionName name of the function to be called
        @param functionParams list of parameters to be passed for the function
        @return value indicating whether the control was successful
        '''
        
        if(functionName == "postureCommand"):
            return self.postureCommand(functionParams)
        
        elif(functionName == "sayHello"):
            return self.sayHello()
        
        elif(functionName == "sayOwnName"):
            return self.sayOwnName()
        
        elif(functionName == "ledShow"):
            return self.ledShow()
        

 
    def postureCommand(self, params):
        if len(params) != 1:
            print "postureCommand, bad arguments!"
            return False
        
        successfulPosture = self.postureProxy.goToPosture(params[0], 1.0)
        self.motionProxy.rest();
        return successfulPosture
        
        
    
    def blinkEyesErrorResponse(self):
        self.ledProxy.fadeRGB("FaceLeds",1,0,0,0.1)
        time.sleep(0.5)
        self.ledProxy.fadeRGB("FaceLeds",1,1,1,0.1)

        
    def blinkEyes(self, waitBeforeBlink = 0):
        if waitBeforeBlink:
            time.sleep(waitBeforeBlink)
        
        self.ledProxy.off("FaceLeds")
        time.sleep(0.2)
        self.ledProxy.on("FaceLeds")
        
    def ledShow(self, duration = 5):
        self.ledProxy.rasta(duration)
        
    def sayHello(self):
        # Have to speak English because no Finnish language package currently installed in Nao
        self.textToSpeechProxy.say("Hello")
        return True
        
    def sayOwnName(self):
        self.textToSpeechProxy.say("My favourite movie is Terminator")
        return True
    
    
        

if __name__ == "__main__":
    '''
    Main for testing the usage from command line       
    '''

    if len(sys.argv) < 4:
        print "Usage <robot ip> <command> <proxyName>"
    else:
        robotIp = sys.argv[1]
        command = sys.argv[2]
        proxyName = sys.argv[3]
        try:
            pythonBroker = ALBroker("pythonBroker","0.0.0.0",9999, robotIp,9559)
            
            controller = ControlNaoModule("NaoController", robotIp)
            #controller.blinkEyesErrorResponse()
            #controller.randomEyes(3)
            #controller.sayHello()
            #controller.sayOwnName()
            controller.executeControl("postureCommand", ["StandZero"])
            '''
            succeedPosture = controller.executeControl(command, proxyName)
            print "Control execution successfull?", succeedPosture
     
            pythonBroker.shutdown()
            '''
            
        except Exception, e:
            print "Error occured", e
        
               



